#ifndef MATRIX_H_
#define MATRIX_H_

#include "om_vector.h"
#include "om_comparison.h"

namespace om
{

template<int SIZE>
class matrix
{
public:
	typedef vector<SIZE> row;

	enum {size=SIZE};

	matrix() : m_(size)
	{
	}

	row operator[](int i) const { return m_[i]; }

	row& operator[](int i) { return m_[i]; }

	row operator*(const row& v) const
	{
		row r;
		for(int i=0;i<size;i++) { r[i]=(*this)[i]*v; }
		return r;
	}

	matrix operator*(const matrix& m) const
	{
		matrix r;
		for(int i=0;i<size;i++)
		{
			for(int j=0;j<size;j++)
			{
				for(int k=0;k<size;k++)
				{
					r[i][j]+=(*this)[i][k]*m[k][j];
				}
			}
		}
		return r;
	}

	std::string to_string() const
	{
		std::ostringstream output;
		for(int i=0;i<size;i++) { output << (*this)[i].to_string() << (i+1<size ? "\n" : ""); }
		return output.str();
	}

	static matrix from_stream(std::istream& input)
	{
		matrix m;
		for(int i=0;i<size;i++) { m[i]=row::from_stream(input); }
		return m;
	}

	static matrix identity()
	{
		matrix m;
		for(int i=0;i<size;i++) { m[i][i]=1; };
		return m;
	}

	bool is_identity() const
	{
		for(int i=0;i<size;i++) {
			for(int j=0;j<size;j++) {
				if((i!=j && neq((*this)[i][j], 0)) || (i==j && neq((*this)[i][j],1))) { return false; } } }
		return true;
	}

	template<typename T>
	static std::pair<bool, T> apply_gauss_jordan_elimination(const matrix& left, const T& right)
	{
		bool valid=true;
		matrix a=left;
		T b=right;
		for(int i=0;i<size && valid;i++)
		{
			if(eq(a[i][i], 0.0))
			{
				int l=i+1;
				while(l<size && eq(a[l][i], 0)) { l++; }
				if(l<size)
				{
					std::swap(a[i], a[l]);
					std::swap(b[i], b[l]);
				}
				else { valid=false; }
			}
			if(valid)
			{
				const double divisor=a[i][i];
				a[i]=a[i]/divisor;
				b[i]=b[i]/divisor;
				for(int k=i+1;k<size;k++)
				{
					const double multiplicator=a[k][i];
					a[k]=a[k]-(a[i]*multiplicator);
					b[k]=b[k]-(b[i]*multiplicator);
				}
			}
		}
		for(int i=size-1;i-1>=0 && valid;i--)
		{
			for(int k=i-1;k>=0;k--)
			{
				const double multiplicator=a[k][i];
				a[k]=a[k]-(a[i]*multiplicator);
				b[k]=b[k]-(b[i]*multiplicator);
			}
		}
		return std::make_pair(valid && a.is_identity(), b);
	}

	std::vector<row> m_;
};

}


#endif /* MATRIX_H_ */
